#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/pfsm_free.h"
#include "axis/axis.h"
#include "kine/kine.h"
#include "axisgroup/axisgroup_set.h"
#include <math.h>

#include <thread>

#define TEST_COUNT 10

using namespace axisgroup;

class MockAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
};

class FreeModeTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        for (int i = 0; i < 7; i++) { EXPECT_CALL(joint[i], get_id()).WillRepeatedly(testing::Return(i)); }
    }
    virtual void TearDown() override {}

    MockAxis joint[7];
    pid_t pid_;
    int shm_id_ = -1;
};

TEST_F(FreeModeTest, JOG_JOINT)
{
    std::shared_ptr<Robot> axis_grp = std::make_shared<Robot>();
    axis_grp->reset();
    axis_grp->change_to_booting();
    ASSERT_EQ(axis_grp->link_joint(&joint[0]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[1]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[2]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[3]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[4]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[5]), 0);
    ASSERT_EQ(axis_grp->joint_num(), 6);

    auto free = std::make_shared<PFsmFree>(axis_grp.get());
    free->switch_jog_type(PFsmFree::JOG_JOINT);
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_JOINT);

    double target_v = 5;
    double target_a = 100;
    double target_s = 20;
    // 测试单轴jog
    joint[0].set_max_pos(100);
    joint[0].set_max_vel(target_v);
    joint[0].set_max_acc(80);

    free->free_tp_[0].enable = 1;
    free->free_tp_[0].pos_cmd = target_s;
    free->free_tp_[0].max_vel = 20;
    free->free_tp_[0].max_acc = target_a;

    AxisGroupPose pose = axis_grp->get_cur_cmd_pos();
    ASSERT_DOUBLE_EQ(pose.jpos[0], 0);

    double t = 0;
    // 估计运动时间
    if (target_s > target_v * target_v / target_a)
    {
        t = (target_s - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(target_s * target_v);
    }

    int i = 0;
    for (; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_[0].curr_vel) < 1e-4)
                break;
        }
    }
    EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[0], target_s, 1e-3);
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
}

TEST_F(FreeModeTest, JOG_JOINT_MULTI)
{
    std::shared_ptr<Robot> axis_grp = std::make_shared<Robot>();
    axis_grp->reset();
    axis_grp->change_to_booting();
    ASSERT_EQ(axis_grp->link_joint(&joint[0]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[1]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[2]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[3]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[4]), 0);
    ASSERT_EQ(axis_grp->link_joint(&joint[5]), 0);
    ASSERT_EQ(axis_grp->joint_num(), 6);
    axis_grp->change_fsm_stat(axisgroup::AxisGroup::FsmState::Standby);

    auto free = std::make_shared<PFsmFree>(axis_grp.get());

    // 初始化DH参数
    double a[6] = {0, 0, 0.595, 0.5715, 0, 0};
    double d[6] = {0.14265, 0, 0, -0.1315, 0.115, 0.1035};
    double alpha[6] = {0, M_PI / 2, 0, 0, M_PI / 2, -M_PI / 2};
    double offset[6] = {};

    RobotDH dh = {};
    for (int i = 0; i < 6; i++)
    {
        dh.a[i] = a[i] * 1000.0;
        dh.d[i] = d[i] * 1000.0;
        dh.alpha[i] = alpha[i];
        dh.joint_homeoff[i] = offset[i];
    }
    axis_grp->get_set_intf()->set_axisgroup_dh(dh);

    free->switch_jog_type(PFsmFree::JOG_JOINT);
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_JOINT);

    double target_v = 5;
    double target_a = 100;
    double target_s = 90;
    // 测试多轴jog
    for (int i = 0; i < 6; i++)
    {
        joint[i].set_max_pos(100);
        joint[i].set_max_vel(target_v);
        joint[i].set_max_acc(80);

        free->free_tp_[i].enable = 1;
        free->free_tp_[i].pos_cmd = target_s;
        free->free_tp_[i].max_vel = 20;
        free->free_tp_[i].max_acc = target_a;
    }

    AxisGroupPose pose = axis_grp->get_cur_cmd_pos();
    ASSERT_DOUBLE_EQ(pose.jpos[0], 0);

    double t = 0;
    // 估计运动时间
    if (target_s > target_v * target_v / target_a)
    {
        t = (target_s - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(target_s * target_v);
    }

    int i = 0;
    for (; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_[0].curr_vel) < 1e-4)
                break;
        }
    }
    for (int i = 0; i < 6; i++) { EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[i], target_s, 1e-3); }
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
    auto res = axis_grp->cmd_rst_;
    printf("Now : %lf, %lf, %lf, %lf, %lf, %lf\n", res.pose.tran.x, res.pose.tran.y, res.pose.tran.z, res.pose.a, res.pose.b, res.pose.c);

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // 转笛卡尔空间运动
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    free->switch_jog_type(PFsmFree::JOG_CART_WORLD);
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_CART_WORLD);

    for (auto tp : free->free_tp_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，关节运动全部停止
    for (auto tp : free->free_tp_cart_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，笛卡尔运动全部停止

    for (int i = 0; i < 6; i++)
    {
        if (i < 3)
        {
            EXPECT_NEAR(free->free_tp_cart_[i].curr_pos, axis_grp->get_cur_cmd_pos().pose_array()[i], 1e-5);  // 进入后curr_pos为当前位置
        }
        else
        {
            EXPECT_NEAR(free->free_tp_cart_[i].curr_pos, 0, 1e-5);  // 进入后curr_pos为当前位置
        }
        EXPECT_EQ(free->free_tp_cart_[i].curr_vel, 0);
    }

    auto cart_tp = &(free->free_tp_cart_[0]);

    target_v = 5;
    target_a = 100;
    target_s = cart_tp->curr_pos + 90;
    cart_tp->enable = 1;
    cart_tp->max_vel = target_v;
    cart_tp->max_acc = target_a;
    cart_tp->pos_cmd = target_s;

    t = 0;
    double ds = 90;
    // 估计运动时间
    if (ds > target_v * target_v / target_a)
    {
        t = (ds - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(ds * target_a) / target_a * 2;
    }

    for (i = 0; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_cart_[0].curr_vel) < 1e-4)
                break;
        }
        // printf("Now Cart1: %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        axis_grp->get_cur_cmd_pos().pose_array()[0],
        //        axis_grp->get_cur_cmd_pos().pose_array()[1],
        //        axis_grp->get_cur_cmd_pos().pose_array()[2],
        //        axis_grp->get_cur_cmd_pos().pose_array()[3],
        //        axis_grp->get_cur_cmd_pos().pose_array()[4],
        //        axis_grp->get_cur_cmd_pos().pose_array()[5]);
        // printf("Here joint: %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        axis_grp->get_cur_cmd_pos().jpos[0],
        //        axis_grp->get_cur_cmd_pos().jpos[1],
        //        axis_grp->get_cur_cmd_pos().jpos[2],
        //        axis_grp->get_cur_cmd_pos().jpos[3],
        //        axis_grp->get_cur_cmd_pos().jpos[4],
        //        axis_grp->get_cur_cmd_pos().jpos[5]);
    }
    EXPECT_NEAR(axis_grp->get_cur_cmd_pos().pose_array()[0], target_s, 1e-3);
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
    double target_q[6] = {97.564, 88.899, 91.090, 90.011, 90.000000, 82.4360};
    for (int i = 0; i < 6; i++) { EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[i], target_q[i], 1e-3); }
    res = axis_grp->cmd_rst_;
    printf("Now : %lf, %lf, %lf, %lf, %lf, %lf\n", res.pose.tran.x, res.pose.tran.y, res.pose.tran.z, res.pose.a, res.pose.b, res.pose.c);
    printf("Here joint: %lf, %lf, %lf, %lf, %lf, %lf\n",
           axis_grp->get_cur_cmd_pos().jpos[0],
           axis_grp->get_cur_cmd_pos().jpos[1],
           axis_grp->get_cur_cmd_pos().jpos[2],
           axis_grp->get_cur_cmd_pos().jpos[3],
           axis_grp->get_cur_cmd_pos().jpos[4],
           axis_grp->get_cur_cmd_pos().jpos[5]);

    cart_tp->enable = 0;                              // 关闭
                                                      // 验证姿态运动
    free->switch_jog_type(PFsmFree::JOG_CART_WORLD);  // 需要再进一遍，以更新基准位置
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_CART_WORLD);
    int dir = 3;
    cart_tp = &(free->free_tp_cart_[dir]);

    target_v = 5;
    target_a = 100;
    target_s = 10;
    cart_tp->enable = 1;
    cart_tp->max_vel = target_v;
    cart_tp->max_acc = target_a;
    cart_tp->pos_cmd = target_s;

    t = 0;
    ds = 10;
    // 估计运动时间
    if (ds > target_v * target_v / target_a)
    {
        t = (ds - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(ds * target_a) / target_a * 2;
    }

    for (i = 0; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_cart_[dir].curr_vel) < 1e-4)
                break;
        }
        // printf("Now Cart2: %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        axis_grp->get_cur_cmd_pos().pose_array()[0],
        //        axis_grp->get_cur_cmd_pos().pose_array()[1],
        //        axis_grp->get_cur_cmd_pos().pose_array()[2],
        //        axis_grp->get_cur_cmd_pos().pose_array()[3],
        //        axis_grp->get_cur_cmd_pos().pose_array()[4],
        //        axis_grp->get_cur_cmd_pos().pose_array()[5]);
    }
    // EXPECT_NEAR(axis_grp->get_cur_cmd_pos().pose_array()[dir], target_s, 1e-3);
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
    // printf("Here joint: %lf, %lf, %lf, %lf, %lf, %lf\n",
    //        axis_grp->get_cur_cmd_pos().jpos[0],
    //        axis_grp->get_cur_cmd_pos().jpos[1],
    //        axis_grp->get_cur_cmd_pos().jpos[2],
    //        axis_grp->get_cur_cmd_pos().jpos[3],
    //        axis_grp->get_cur_cmd_pos().jpos[4],
    //        axis_grp->get_cur_cmd_pos().jpos[5]);
    // double target_q[6] = {97.564, 88.899, 91.090, 90.011, 90.000000, 82.4360};
    // for (int i = 0; i < 6; i++) { EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[i], target_q[i], 1e-3); }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // 转笛卡尔工具坐标系运动
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    free->switch_jog_type(PFsmFree::JOG_CART_TOOL);
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_CART_TOOL);

    for (auto tp : free->free_tp_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，关节运动全部停止
    for (auto tp : free->free_tp_cart_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，笛卡尔运动全部停止

    for (int i = 0; i < 6; i++)
    {
        EXPECT_NEAR(free->free_tp_cart_[i].curr_pos, 0, 1e-5);  // 进入后curr_pos为当前位置
        EXPECT_EQ(free->free_tp_cart_[i].curr_vel, 0);
    }

    cart_tp = &(free->free_tp_cart_[0]);
    dir = 0;

    target_v = 50;
    target_a = 100;
    target_s = 200;
    cart_tp->enable = 1;
    cart_tp->max_vel = target_v;
    cart_tp->max_acc = target_a;
    cart_tp->pos_cmd = target_s;

    t = 0;
    ds = target_s;
    // 估计运动时间
    if (ds > target_v * target_v / target_a)
    {
        t = (ds - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(ds * target_a) / target_a * 2;
    }

    for (i = 0; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_cart_[dir].curr_vel) < 1e-4)
                break;
        }
        // printf("Now Cart3: %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        axis_grp->get_cur_cmd_pos().pose_array()[0],
        //        axis_grp->get_cur_cmd_pos().pose_array()[1],
        //        axis_grp->get_cur_cmd_pos().pose_array()[2],
        //        axis_grp->get_cur_cmd_pos().pose_array()[3],
        //        axis_grp->get_cur_cmd_pos().pose_array()[4],
        //        axis_grp->get_cur_cmd_pos().pose_array()[5]);
    }
    // EXPECT_NEAR(axis_grp->get_cur_cmd_pos().pose_array()[0], target_s, 1e-3);
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
    // printf("Here joint: %lf, %lf, %lf, %lf, %lf, %lf\n",
    //        axis_grp->get_cur_cmd_pos().jpos[0],
    //        axis_grp->get_cur_cmd_pos().jpos[1],
    //        axis_grp->get_cur_cmd_pos().jpos[2],
    //        axis_grp->get_cur_cmd_pos().jpos[3],
    //        axis_grp->get_cur_cmd_pos().jpos[4],
    //        axis_grp->get_cur_cmd_pos().jpos[5]);
    double target_q3[6] = {101.098603, 68.147063, 101.911577, 109.758001, 91.915580, 79.065906};
    for (int i = 0; i < 6; i++) { EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[i], target_q3[i], 1e-3); }

    // 切换为tool姿态
    free->switch_jog_type(PFsmFree::JOG_CART_TOOL);
    ASSERT_EQ(free->get_free_type(), PFsmFree::JOG_CART_TOOL);

    for (auto tp : free->free_tp_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，关节运动全部停止
    for (auto tp : free->free_tp_cart_)
    {
        EXPECT_FALSE(tp.active);
        EXPECT_FALSE(tp.enable);
    }  // 进入后，笛卡尔运动全部停止

    for (int i = 0; i < 6; i++)
    {
        EXPECT_NEAR(free->free_tp_cart_[i].curr_pos, 0, 1e-5);  // 进入后curr_pos为当前位置
        EXPECT_EQ(free->free_tp_cart_[i].curr_vel, 0);
    }

    dir = 5;
    cart_tp = &(free->free_tp_cart_[dir]);

    target_v = 50;
    target_a = 100;
    target_s = 20;
    cart_tp->enable = 1;
    cart_tp->max_vel = target_v;
    cart_tp->max_acc = target_a;
    cart_tp->pos_cmd = target_s;

    t = 0;
    ds = target_s;
    // 估计运动时间
    if (ds > target_v * target_v / target_a)
    {
        t = (ds - target_v * target_v / target_a) / target_v + 2 * target_v / target_a;
    }
    else
    {
        t = sqrt(ds * target_a) / target_a * 2;
    }

    for (i = 0; i < 10000; i++)
    {
        axisgroup::PlanStatus res = free->update();
        ASSERT_EQ(res.errcode, 0);
        if ((i + 1) * 0.008 < t)
        {
            // EXPECT_FALSE(axis_grp->get_cmd_inpos());
        }
        else
        {
            if (fabs(free->free_tp_cart_[dir].curr_vel) < 1e-4)
                break;
        }
        // printf("Now Cart4: %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        axis_grp->get_cur_cmd_pos().pose_array()[0],
        //        axis_grp->get_cur_cmd_pos().pose_array()[1],
        //        axis_grp->get_cur_cmd_pos().pose_array()[2],
        //        axis_grp->get_cur_cmd_pos().pose_array()[3],
        //        axis_grp->get_cur_cmd_pos().pose_array()[4],
        //        axis_grp->get_cur_cmd_pos().pose_array()[5]);
    }
    // EXPECT_NEAR(axis_grp->get_cur_cmd_pos().pose_array()[0], target_s, 1e-3);
    // EXPECT_TRUE(axis_grp->get_cmd_inpos());
    printf("desired time %lf, real time %lf\n", t, (i + 1) * 0.008);
    // printf("Here joint: %lf, %lf, %lf, %lf, %lf, %lf\n",
    //        axis_grp->get_cur_cmd_pos().jpos[0],
    //        axis_grp->get_cur_cmd_pos().jpos[1],
    //        axis_grp->get_cur_cmd_pos().jpos[2],
    //        axis_grp->get_cur_cmd_pos().jpos[3],
    //        axis_grp->get_cur_cmd_pos().jpos[4],
    //        axis_grp->get_cur_cmd_pos().jpos[5]);
    double target_q4[6] = {101.098603, 68.147063, 101.911577, 109.758001, 91.915580, 99.065906};
    for (int i = 0; i < 6; i++) { EXPECT_NEAR(axis_grp->get_cur_cmd_pos().jpos[i], target_q4[i], 1e-3); }
}